import time
import math

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
 
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
import tf_transformations

from geometry_msgs.msg import Quaternion
from my_action.action import JointTarget
# w, x, y, z
movingBindings = {
    'w': (0, 1, 0, 0), #EXTEND FOWRAWD
    's': (0, -1, 0, 0), #EXTEND BACK
    'a': (0, 0, 1, 0), #ROTATE LEFT
    'd': (0, 0, -1, 0), #ROTATE RIGHT
    'u': (1, 0, 0, 0), #RISE UP
    'j': (-1, 0, 0, 0), #RISE DOWN
    'i': (0, 0, 0, 1), #LIFT UP
    'k': (0, 0, 0, -1), #LIFT DOWN
}
 
class JointServer(Node):

    def __init__(self):
        super().__init__('joint_server')
        self._action_server = ActionServer(
            self,
            JointTarget,
            'joint_target',
            execute_callback=self.execute_callback,
            cancel_callback=self.cancel_callback)
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.publisher_ = self.create_publisher(Quaternion, '/cranebot/move', 10)

    def get_tf(self, j1, j2):
        tf =  self.tf_buffer.lookup_transform(j1, j2, rclpy.time.Time())
        return tf

    def transform_joint_distance(self, joint_type):
        if joint_type == "raise":
            trans = self.get_tf("arm_throttle", "cab")
            euler = tf_transformations.euler_from_quaternion([
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w     
            ])
            distance = euler[1]/math.pi*180

        elif joint_type == "extend":
            trans = self.get_tf("arm_throttle", "arm_end")
            distance = trans.transform.translation.x * 100

        elif joint_type == "rotate":
            trans = self.get_tf("cab", "car")
            euler = tf_transformations.euler_from_quaternion([
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w     
            ])
            distance = euler[2]/math.pi*180

        elif joint_type == "lift":
            trans = self.get_tf("arm_end", "hook")
            distance = trans.transform.translation.z * 100

        return distance

# 从移动距离和关节类型获取应当执行的key操作
    def getKey(self, move, joint_type):
        if joint_type == "raise":  
            if move > 0:
                moving_key = 'u'
            else:
                moving_key = 'j'

        elif joint_type == "extend":
            if move > 0:
                moving_key = 'w'
            else:
                moving_key = 's'

        elif joint_type == "rotate":
            if move > 0:
                moving_key = 'd'
            else:
                moving_key = 'a'

        elif joint_type == "lift":
            if move > 0:
                moving_key = 'i'
            else:
                moving_key = 'k'

        return moving_key

# 获取当前关节位置，与client的target进行比对，如果尚未接近，则开关不变。到达位置后，控制继电器改为停止模式。
    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')
        feedback_msg = JointTarget.Feedback()
        feedback_msg.distance_to_target = goal_handle.request.target
        joint_type = goal_handle.request.joint_type
        init_position = self.transform_joint_distance(joint_type)
        required_position =  init_position + goal_handle.request.target
        moving_distance = required_position - init_position
        moving_key = self.getKey(goal_handle.request.target, joint_type)
        msg = Quaternion()
        if moving_key in movingBindings.keys():
            msg.w = float(movingBindings[moving_key][0])
            msg.x = float(movingBindings[moving_key][1])
            msg.y = float(movingBindings[moving_key][2])
            msg.z = float(movingBindings[moving_key][3])
        self.publisher_.publish(msg)
        self.get_logger().info(str(msg))
        now_position = self.transform_joint_distance(joint_type)

        while (abs(now_position - required_position) > 0.1):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('Goal canceled.')
                JointTarget.Result().move_distance = movement
                msg.w = 0.0
                msg.x = 0.0
                msg.y = 0.0
                msg.z = 0.0
                self.publisher_.publish(msg)
                self.get_logger().info("Stop command sent.")
                return JointTarget.Result()

            movement = now_position - init_position
            self.get_logger().info('The ' + joint_type + ' joint is moved by' +str(movement))
            feedback_msg.distance_to_target = required_position - now_position
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.1)
            now_position = self.transform_joint_distance(joint_type)

        goal_handle.succeed()
        msg.w = 0.0
        msg.x = 0.0
        msg.y = 0.0
        msg.z = 0.0
        self.publisher_.publish(msg)
        self.get_logger().info("Accessed"+str(msg))
        result = JointTarget.Result()
        result.move_distance = now_position - init_position
        self.get_logger().info('Result: The joint moves by: {0}'.format(result.move_distance))

        return result

    def cancel_callback(self, goal_handle):
        """Accept or reject a client request to cancel an action."""
        self.get_logger().info('Received cancel request')
        return CancelResponse.ACCEPT

def main(args=None):
    rclpy.init(args=args)
 
    joint_server = JointServer()
    executor = MultiThreadedExecutor()
    rclpy.spin(joint_server, executor=executor)
    joint_server.destroy()
    rclpy.shutdown()
 
 
if __name__ == '__main__':
    main()